#include "gimbal_task.h"
#include "CAN_receive.h"
#include "INS_task.h"
#include "NUC_communicate.h"
#include "OLED.h"
#include "arm_math.h"
#include "bsp_led.h"
#include "cmsis_os.h"
#include "detect_task.h"
#include "main.h"
#include "pid.h"
#include "remote_control.h"
#include "robot_config.h"
#include "robot_total_mode_task.h"
#include "user_lib.h"
#include "struct_typedef.h"
#include "referee.h"
#include "debug_config.h"

#ifdef GIMBAL_WITHOUT_IMU
#define GIMBAL_TASK_INIT_TIME 2000
#define GIMBAL_TASK_CTRL_TIME 2

#define SAFE_AUTO_SHIFT_TIME 1500 // 自动模式时，当装甲板丢失时，使用手动控制，但不清空操作手鼠标偏移量。当重新从手动进入自动模式模式时，操作手可能因为来不及反应,导致偏移量破坏。因此设计安全时间，当重新进入时，有一段时间无视操作手新增偏移量。这段时间为SAFE_AUTO_SHIFT_TIME 单位ms

#define MID_ECD_OF_PITCH 5360 //pitch 0度时ecd
#define PITCH_ECD_MAX 5330 //仰头到最高
#define PITCH_ECD_MIN 5746
#define PITCH_ECD_TO_RAD -ECD_TO_RAD //用于改变符号//为负数，发送时加负号
#define PITCH_RPM_TO_RAD -RPM_TO_RAD    
#define PITCH_TEST_RAD_MIN (((fp32)(PITCH_ECD_MIN-MID_ECD_OF_PITCH))*PITCH_ECD_TO_RAD)
#define PITCH_TEST_RAD_MAX (((fp32)(PITCH_ECD_MAX-MID_ECD_OF_PITCH))*PITCH_ECD_TO_RAD)

#define MID_ECD_OF_YAW 4870
#define YAW_ECD_TO_RAD ECD_TO_RAD //用于改变符号
#define YAW_RPM_TO_RAD RPM_TO_RAD

#ifdef FULL_ROUNDS
#define YAW_MAX_RANGE ECD_FULL_ROUND
#define YAW_RECOVER_POINT  0 //0.5f*ECD_FULL_ROUND//当转过的角度大于YAW_MAX_RANGE后进入恢复旋转状态，不管什么样的角度都往解开线的方向转，越过YAW_RECOVER_POINT后可以自然向合法的方向转动。
//为什么要取这样一个值，想像一下敌人在转到一圈和不到一圈的地方反复横跳，这是保证只转一次优弧
#endif
#ifdef LIMIT_ROUND
#define YAW_MAX_RANGE PI/6
#endif
/**********************PID控制参数********************/
#define PITCH_AGL_KP 15.0f
#define PITCH_AGL_KI 0.0f
#define PITCH_AGL_KD 0.0f
#define PITCH_AGL_SPD_MAX_OUT (30.0f)
#define PITCH_AGL_SPD_MAX_IOUT (0.7f)

#define PITCH_SPD_KP 1200.0f
#define PITCH_SPD_KI 0.0f
#define PITCH_SPD_KD 0.0f
#define PITCH_VOLT_MAX_OUT 30000.0f
#define PITCH_VOLT_MAX_IOUT 5000.0f

#define YAW_AGL_KP 15.0f
#define YAW_AGL_KI 0.0f
#define YAW_AGL_KD 0.0f
#define YAW_AGL_SPD_MAX_OUT (50.0f)
#define YAW_AGL_SPD_MAX_IOUT (0.7f)

#define YAW_SPD_KP 1300.0f
#define YAW_SPD_KI 0000.0f
#define YAW_SPD_KD 000.0f
#define YAW_VOLT_MAX_OUT 30000.0f
#define YAW_VOLT_MAX_IOUT 5000.0f


// 一阶滤波器的参数,专门给。这两个值越大，理论上越丝滑
#define PITCH_SPD_FILTER_NUM 15.0f
#define YAW_SPD_FILTER_NUM 20.0f

#define rc_deadband_limit(input, output, dealine)        \
    {                                                    \
        if ((input) > (dealine) || (input) < -(dealine)) \
        {                                                \
            (output) = (input);                          \
        }                                                \
        else                                             \
        {                                                \
            (output) = 0;                                \
        }                                                \
    }

#define range_limit_inside(toBeLimited, range) \
    {                                          \
        if ((toBeLimited) > (range))           \
        {                                      \
            (toBeLimited) = (range);           \
        }                                      \
        else if ((toBeLimited) < -(range))     \
        {                                      \
            (toBeLimited) = -(range);          \
        }                                      \
    }

struct gimbalMotorCtrl_t
{
    // rads
    fp32 nowAbsoluteAngle;    // 当前姿态角
    fp32 radSpeed;            // 角速度 rad/s
    fp32 wantedAbsoluteAngle; // 目标姿态角
    // PIDs
    pid_type_def spdPid; // 用于控制速度的PID环。输入角速度 rad/s, 输出GM6020电压 int16
    pid_type_def aglPid; // 用于控制角度的PID环。输入角度 rad,输出角速度rad/s
    // 控制量
    int16_t giveVolt; // GM6020发送电压
    uint8_t zeroVoltMark;
    // 一阶滤波器，过滤角速度
    first_order_filter_type_t spd_filter;
};

#ifdef FULL_ROUNDS
typedef struct{
    int16_t nowRounds;
    int16_t lastEcd;
    int16_t stepEcd;
    int8_t nextSpeedDirection; //下一次计算时速度的方向，0表示不强制改变方向
}roundsMonitor_t;
#endif

static struct gimbalMotorCtrl_t gimbalPitchCtrl, gimbalYawCtrl;
static fp32 yawSpdPID[3] = {YAW_SPD_KP, YAW_SPD_KI, YAW_SPD_KD};
static fp32 pitchSpdPID[3] = {PITCH_SPD_KP, PITCH_SPD_KI, PITCH_SPD_KD};
static fp32 yawAglPID[3] = {YAW_AGL_KP, YAW_AGL_KI, YAW_AGL_KD};
static fp32 pitchAglPID[3] = {PITCH_AGL_KP, PITCH_AGL_KI, PITCH_AGL_KD};
// 控制指针
static const uint8_t *robotMode;
static const RC_ctrl_t *rc_p;  // 遥控器位置指针，需要初始化
static const fromNUC_t *nuc_p; // NUC数据位置。
static const remote_control_t* remoteControl_p; //图传链路控制
static const motor_measure_t* yawUpMotorMeasure_p;
static const motor_measure_t* pitchUpMotorMeasure_p;
#ifdef FULL_ROUNDS
static roundsMonitor_t yawRoundsMonitor;
#endif

typedef struct
{
    fp32 LastP; // 上次估算协方差 初始化值为0.02
    fp32 Now_P; // 当前估算协方差 初始化值为0
    fp32 out;   // 卡尔曼滤波器输出 初始化值为0
    fp32 Kg;    // 卡尔曼增益 初始化值为0
    fp32 Q;     // 过程噪声协方差 初始化值为0.00001
    fp32 R;     // 观测噪声协方差 初始化值为0.00003
} KFP_t;        // Kalman Filter parameter

static KFP_t KfpPitchWantAgl = {0.02, 0, 0, 0.9, 0.0001, 0.0003};
static KFP_t KfpYawWantAgl = {0.02, 0, 0, 0.9, 0.0001, 0.0003};

static fp32 kalmanFilter(KFP_t *kfp, fp32 input)
{
    // 预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
    kfp->Now_P = kfp->LastP + kfp->Q;
    // 卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
    //  kfp->Kg = kfp->Now_P / (kfp->Now_P + kfp->R);

    // 更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
    kfp->out = kfp->out + kfp->Kg * (input - kfp->out); // 因为这一次的预测值就是上一次的输出值
    // 更新协方差方程: 本次的系统协方差付给 kfp->LastP 威下一次运算准备。
    kfp->LastP = (1 - kfp->Kg) * kfp->Now_P;
    return kfp->out;
}

static void init_gimbal_ctrls(void)
{
//    uint8_t i;

    // 初始化PID参数
    // 两个双环PID不共用参数
    PID_init(&(gimbalPitchCtrl.spdPid), PID_POSITION, pitchSpdPID, PITCH_VOLT_MAX_OUT, PITCH_VOLT_MAX_IOUT);
    PID_init(&(gimbalYawCtrl.spdPid), PID_POSITION, yawSpdPID, YAW_VOLT_MAX_OUT, YAW_VOLT_MAX_IOUT);

    PID_init(&(gimbalPitchCtrl.aglPid), PID_POSITION, pitchAglPID, PITCH_AGL_SPD_MAX_OUT, PITCH_AGL_SPD_MAX_IOUT);
    PID_init(&(gimbalYawCtrl.aglPid), PID_POSITION, yawAglPID, YAW_AGL_SPD_MAX_OUT, YAW_AGL_SPD_MAX_IOUT);

    // 滤波初始化
    const static fp32 gimbal_pitch_order_filter[1] = {PITCH_SPD_FILTER_NUM};
    const static fp32 gimbal_yaw_order_filter[1] = {YAW_SPD_FILTER_NUM};

    first_order_filter_init(&(gimbalPitchCtrl.spd_filter), GIMBAL_TASK_CTRL_TIME, gimbal_pitch_order_filter);
    first_order_filter_init(&(gimbalYawCtrl.spd_filter), GIMBAL_TASK_CTRL_TIME, gimbal_yaw_order_filter);

    // 初始化当前角度和目标角度、pid参数
    gimbalYawCtrl.zeroVoltMark = 0;
    gimbalYawCtrl.nowAbsoluteAngle = yawUpMotorMeasure_p->ecd*ECD_TO_RAD;
    gimbalYawCtrl.wantedAbsoluteAngle = gimbalYawCtrl.nowAbsoluteAngle;
    gimbalPitchCtrl.zeroVoltMark = 0;
    gimbalPitchCtrl.nowAbsoluteAngle = pitchUpMotorMeasure_p->ecd*PITCH_ECD_TO_RAD;
    gimbalPitchCtrl.wantedAbsoluteAngle = gimbalPitchCtrl.nowAbsoluteAngle;
    PID_clear(&gimbalYawCtrl.aglPid);
    PID_clear(&gimbalYawCtrl.spdPid);
    PID_clear(&gimbalPitchCtrl.aglPid);
    PID_clear(&gimbalPitchCtrl.spdPid);
    gimbalYawCtrl.radSpeed = 0;
    gimbalPitchCtrl.radSpeed = 0;
}

static fp32 radFormat(fp32 rawAngle)
{
    while (rawAngle > PI)
        rawAngle -= (2 * PI);
    while (rawAngle < (-PI))
        rawAngle += (2 * PI);
    return rawAngle;
}

static fp32 gimbal_PID_calc(pid_type_def *pid, fp32 ref, fp32 set)
{
    if (pid == NULL)
    {
        return 0.0f;
    }

    pid->error[2] = pid->error[1];
    pid->error[1] = pid->error[0];
    pid->set = set;
    pid->fdb = ref;
    pid->error[0] = radFormat(set - ref);
    if (pid->mode == PID_POSITION)
    {
        pid->Pout = pid->Kp * pid->error[0];
        pid->Iout += pid->Ki * pid->error[0];
        pid->Dbuf[2] = pid->Dbuf[1];
        pid->Dbuf[1] = pid->Dbuf[0];
        pid->Dbuf[0] = (pid->error[0] - pid->error[1]);
        pid->Dout = pid->Kd * pid->Dbuf[0];
        range_limit_inside(pid->Iout, pid->max_iout);
        pid->out = pid->Pout + pid->Iout + pid->Dout;
        range_limit_inside(pid->out, pid->max_out);
    }
    else if (pid->mode == PID_DELTA)
    {
        pid->Pout = pid->Kp * (pid->error[0] - pid->error[1]);
        pid->Iout = pid->Ki * pid->error[0];
        pid->Dbuf[2] = pid->Dbuf[1];
        pid->Dbuf[1] = pid->Dbuf[0];
        pid->Dbuf[0] = (pid->error[0] - 2.0f * pid->error[1] + pid->error[2]);
        pid->Dout = pid->Kd * pid->Dbuf[0];
        pid->out += pid->Pout + pid->Iout + pid->Dout;
        range_limit_inside(pid->out, pid->max_out);
    }
    return pid->out;
}

#ifdef STM_STUDIO_UP_GIMBAL
uint16_t yawEcd=0;
uint16_t pitchEcd=0;
#endif

static void refresh_angle_states(void)
{
    // 当前姿态角和角速度
    #ifdef ABSOLUTE_LOCATION
    gimbalYawCtrl.nowAbsoluteAngle =((fp32)(yawUpMotorMeasure_p->ecd-MID_ECD_OF_YAW))*YAW_ECD_TO_RAD+get_now_yaw_angle();
    #endif
    #ifdef RELATIVE_LOCATION
    gimbalYawCtrl.nowAbsoluteAngle =((fp32)(yawUpMotorMeasure_p->ecd-MID_ECD_OF_YAW))*YAW_ECD_TO_RAD;
    #endif
    gimbalYawCtrl.radSpeed = ((fp32)yawUpMotorMeasure_p->speed_rpm)*YAW_RPM_TO_RAD;
    gimbalPitchCtrl.nowAbsoluteAngle = ((fp32)(pitchUpMotorMeasure_p->ecd-MID_ECD_OF_PITCH))*PITCH_ECD_TO_RAD;
    gimbalPitchCtrl.radSpeed = ((fp32)pitchUpMotorMeasure_p->speed_rpm)*PITCH_RPM_TO_RAD;
    #ifdef STM_STUDIO_UP_GIMBAL
    yawEcd=yawUpMotorMeasure_p->ecd;
    pitchEcd=pitchUpMotorMeasure_p->ecd;
    #endif
}

static void get_control_angles(void)
{
    static int16_t yaw_channel = 0, pitch_channel = 0;
    rc_deadband_limit(rc_p->rc.ch[RC_GIMBAL_YAW], yaw_channel, GIMBAL_RC_DEADLINE);
    rc_deadband_limit(rc_p->rc.ch[RC_GIMBAL_PITCH], pitch_channel, GIMBAL_RC_DEADLINE);
    static uint8_t lastMode = ROBOT_STATE_POWERLESS; // 初始化为无力
    if (lastMode != (*robotMode))
    {
        gimbalYawCtrl.wantedAbsoluteAngle = gimbalYawCtrl.nowAbsoluteAngle;
    }
    lastMode = *robotMode;
    gimbalYawCtrl.zeroVoltMark = 0;
    gimbalPitchCtrl.zeroVoltMark = 0;
    if (*robotMode == ROBOT_STATE_POWERLESS)
    {
        gimbalYawCtrl.zeroVoltMark = 1;
        gimbalPitchCtrl.zeroVoltMark = 1;
        gimbalPitchCtrl.wantedAbsoluteAngle = 0;
    }
    else if (*robotMode == ROBOT_STATE_AUTO)
    {
        if (gimbal_NUC_is_available())
        {
            #ifdef USE_USART
            gimbalYawCtrl.wantedAbsoluteAngle = gimbalYawCtrl.nowAbsoluteAngle + nuc_p->yaw * PI / (fp32)INT16_MAX;
            gimbalPitchCtrl.wantedAbsoluteAngle = ((fp32)nuc_p->pitch) * 0.3 / (fp32)INT8_MAX;
            #endif
            #ifdef USE_CAN
            gimbalYawCtrl.wantedAbsoluteAngle = gimbalYawCtrl.nowAbsoluteAngle + nuc_p->yaw / (fp32)(INT16_MAX>>2);
            gimbalPitchCtrl.wantedAbsoluteAngle =gimbalPitchCtrl.nowAbsoluteAngle+ ((fp32)nuc_p->pitch) * 0.3f/ (fp32)INT8_MAX;
            #endif
            gimbal_clear_read_mark();
        }
    }
    else
    {
        #ifdef SENTRY
        if(switch_is_up(rc_p->rc.ch[RC_CHOOSE_Y_OR_W])){
        #endif
        if (!robot_is_auxiliaryTarget()) // 当自瞄取消掉的时候，偏移清零
            // 直接复制手动控制的代码
        gimbalYawCtrl.wantedAbsoluteAngle += yaw_channel * YAW_RC_SEN + remoteControl_p->mouse_x * PITCH_MOUSE_SEN;
        gimbalPitchCtrl.wantedAbsoluteAngle += pitch_channel * PITCH_RC_SEN + remoteControl_p->mouse_y * PITCH_MOUSE_SEN;
        gimbalYawCtrl.wantedAbsoluteAngle = kalmanFilter(&KfpYawWantAgl, gimbalYawCtrl.wantedAbsoluteAngle);
        gimbalPitchCtrl.wantedAbsoluteAngle = kalmanFilter(&KfpPitchWantAgl, gimbalPitchCtrl.wantedAbsoluteAngle);
        #ifdef SENTRY
        }
        #endif
    }
}

static void limit_angles()
{
    struct gimbalMotorCtrl_t *c; 
    #ifdef LIMIT_ROUND
    #ifdef ABSOLUTE_LOCATION
    if(radFormat(gimbalYawCtrl.wantedAbsoluteAngle-get_now_yaw_angle())>YAW_MAX_RANGE){
        gimbalYawCtrl.wantedAbsoluteAngle=get_now_yaw_angle()+YAW_MAX_RANGE;
    }
    if(radFormat(gimbalYawCtrl.wantedAbsoluteAngle-get_now_yaw_angle())<-YAW_MAX_RANGE){
        gimbalYawCtrl.wantedAbsoluteAngle=get_now_yaw_angle()-YAW_MAX_RANGE;
    }
    #endif
    #ifdef RELATIVE_LOCATION
    if(radFormat(gimbalYawCtrl.wantedAbsoluteAngle)>YAW_MAX_RANGE){
        gimbalYawCtrl.wantedAbsoluteAngle=YAW_MAX_RANGE;
    }
    if(radFormat(gimbalYawCtrl.wantedAbsoluteAngle)<-YAW_MAX_RANGE){
        gimbalYawCtrl.wantedAbsoluteAngle=-YAW_MAX_RANGE;
    }
    #endif
    #else
    if (radFormat(gimbalYawCtrl.wantedAbsoluteAngle - gimbalYawCtrl.nowAbsoluteAngle) > (PI / 2))
        gimbalYawCtrl.wantedAbsoluteAngle = gimbalYawCtrl.nowAbsoluteAngle + (PI / 2);
    if (radFormat(gimbalYawCtrl.wantedAbsoluteAngle - gimbalYawCtrl.nowAbsoluteAngle) < (-PI / 2))
        gimbalYawCtrl.wantedAbsoluteAngle = gimbalYawCtrl.nowAbsoluteAngle - (PI / 2);
    #endif
    if (radFormat(gimbalPitchCtrl.wantedAbsoluteAngle) > PITCH_TEST_RAD_MAX)
        gimbalPitchCtrl.wantedAbsoluteAngle = PITCH_TEST_RAD_MAX;
    if (radFormat(gimbalPitchCtrl.wantedAbsoluteAngle) < PITCH_TEST_RAD_MIN)
        gimbalPitchCtrl.wantedAbsoluteAngle = PITCH_TEST_RAD_MIN;
}
static fp32 spd_limit_func(void)
{
    fp32 scale;
    if(gimbalYawCtrl.nowAbsoluteAngle<=(YAW_MAX_RANGE/2)&&gimbalYawCtrl.nowAbsoluteAngle>=(-YAW_MAX_RANGE/2))
    {
        scale=1;
    }
    else
    {
        scale=0.5;
    }
    return scale;
}

static void calc_PID(void)
{
    gimbal_PID_calc(&(gimbalYawCtrl.aglPid), gimbalYawCtrl.nowAbsoluteAngle, gimbalYawCtrl.wantedAbsoluteAngle); // 关乎旋转方向的PID控制器
    #ifdef FULL_ROUNDS
    if(radFormat(gimbalYawCtrl.nowAbsoluteAngle-gimbalYawCtrl.wantedAbsoluteAngle)*yawRoundsMonitor.nextSpeedDirection>PI){//按含转动需要反向且要转过一个优弧
        (gimbalYawCtrl.aglPid).out=yawRoundsMonitor.nextSpeedDirection*YAW_MAX_SPEED;
    }
    #endif
    PID_calc(&(gimbalYawCtrl.spdPid), gimbalYawCtrl.radSpeed, (gimbalYawCtrl.aglPid).out*spd_limit_func());                       // 普通的速度控制环
    gimbalYawCtrl.giveVolt = gimbalYawCtrl.spdPid.out;                                                // 给电压
    if (gimbalYawCtrl.zeroVoltMark == 1)
    {
        gimbalYawCtrl.giveVolt = 0;
    }
    gimbal_PID_calc(&(gimbalPitchCtrl.aglPid), gimbalPitchCtrl.nowAbsoluteAngle, gimbalPitchCtrl.wantedAbsoluteAngle); // 关乎旋转方向的PID控制器
    PID_calc(&(gimbalPitchCtrl.spdPid), gimbalPitchCtrl.radSpeed, (gimbalPitchCtrl.aglPid).out);                       // 普通的速度控制环
    gimbalPitchCtrl.giveVolt = gimbalPitchCtrl.spdPid.out;                                                // 给电压
    if (gimbalPitchCtrl.zeroVoltMark == 1)
    {
        gimbalPitchCtrl.giveVolt = 0;
    }
}
#ifdef FULL_ROUNDS
void yaw_motor_rounds_monitor(void){
    yawRoundsMonitor.stepEcd;
    yawRoundsMonitor.lastEcd=yawUpMotorMeasure_p->ecd;
    while (yawRoundsMonitor.stepEcd<-ECD_FULL_ROUND/2){//按设定转速不可能超过
        yawRoundsMonitor.stepEcd+=ECD_FULL_ROUND;
    }
    while (yawRoundsMonitor.stepEcd>ECD_FULL_ROUND/2){
        yawRoundsMonitor.stepEcd-=ECD_FULL_ROUND;
    }
    yawRoundsMonitor.nowRounds+=yawRoundsMonitor.stepEcd;
    if(yawRoundsMonitor.nextSpeedDirection==0){
        if(yawRoundsMonitor.nowRounds>YAW_MAX_RANGE){
            yawRoundsMonitor.nextSpeedDirection=-1;
        }else if(yawRoundsMonitor.nowRounds<-YAW_MAX_RANGE){
            yawRoundsMonitor.nextSpeedDirection=1;
        }
    }else if(yawRoundsMonitor.nextSpeedDirection==-1){
        if(yawRoundsMonitor.nowRounds<YAW_RECOVER_POINT){
            yawRoundsMonitor.nextSpeedDirection=0;
        }
    }else{
        if(yawRoundsMonitor.nowRounds>YAW_RECOVER_POINT){
            yawRoundsMonitor.nextSpeedDirection=0;
        }
    }
}
#endif

void gimbal_without_IMU_task(void const *pvParameters)
{
    osDelay(GIMBAL_TASK_INIT_TIME);
    robotMode = get_robot_present_mode();
    rc_p = get_remote_control_point(); // 获取遥控器数据和NUC数据指针
    nuc_p = get_nuc_control_point();
    remoteControl_p=get_usart_remote_control_p();
    yawUpMotorMeasure_p=get_motor_measure_point(YAW_UP);
    pitchUpMotorMeasure_p=get_motor_measure_point(PITCH_UP);
    init_gimbal_ctrls();
    while (1)
    {
        #ifdef LED_UP_GIMBAL
        aRGB_led_show(0xFF00FF00);
        #endif
        #ifdef FULL_ROUNDS
        yaw_motor_rounds_monitor();
        #endif
        refresh_angle_states(); // 从传感器获得当前角度数据 
        get_control_angles();   // 从遥控器获得目标角度     
        limit_angles();
        calc_PID(); // 总是控制角度，双环，但测试时需要
        #ifdef ZERO_CURRENT_SAFE_GIMBAL_UP
        gimbalPitchCtrl.giveVolt=0;
        gimbalYawCtrl.giveVolt=0;
        #endif
        osDelay(GIMBAL_TASK_CTRL_TIME);
    }
}

const int16_t* get_pitch_up_volt_p(void){
    return &gimbalPitchCtrl.giveVolt;
}

const int16_t* get_yaw_up_volt_p(void){
    return &gimbalYawCtrl.giveVolt;
}

#ifdef OLED_SENTRY_GIMBAL_WITHOUT_IMU
void oled_gimbal_without_imu(void){
    // OLED_printf(0,0,"YawUpECD:%d YawUpSpeed:%d,%.4f PitchUpEcd:%d PitchUpSpeed:%d,%.4f ",yawUpMotorMeasure_p->ecd,yawUpMotorMeasure_p->speed_rpm,gimbalYawCtrl.radSpeed,pitchUpMotorMeasure_p->ecd,pitchUpMotorMeasure_p->speed_rpm,gimbalPitchCtrl.radSpeed);
    OLED_printf(0,0,"YawUpSpeed:%.4f;YawSend:%d;PitchUpSpeed:%.4f PitchSend:%d",gimbalYawCtrl.radSpeed,gimbalYawCtrl.giveVolt,gimbalPitchCtrl.radSpeed,gimbalPitchCtrl.giveVolt);
    // OLED_printf(0,0," gimbalYawCtrl.radSpeed:%.10f gimbalPitchCtrl.radSpeed:%.10f",gimbalYawCtrl.radSpeed,gimbalPitchCtrl.radSpeed);
    
}
#endif
#endif
